/* ######################################################################### */
/** \file Objects2Real.hpp
 * \brief	This file contains the function that
 *
 *
 * PROJECT:   DATMO ROS NODE
 *
 * Copyright (c) 2011 CAR - Universidad Politécnica de Madrid.
 *
 * \author    Gonzalo Rodriguez \n
 *            Universidad Politécnica de Madrid \n
 *            Grupo de Robótica y Cibernética \n
 * \version   0.01
 * \date      2011-06-23
 *
 * \n \n
 * Versionshistory: \n
 * -----------------
 * - Version 0.01:   Gonzalo RODRIGUEZ         2011-06-23 \n
 *      First .
 *
 ######################################################################### */


#include <math.h>
#include <set>
/* ------------------------------------------------------------------------- */
/**	\namespace DATMO
 *
 *  \brief	This is the namespace of the DATMO ROS NODE
 */
/* ------------------------------------------------------------------------- */

namespace DATMO
{
/* ------------------------------------------------------------------------- */
/**	\class Objects2Real
 *
 *  \brief  This is the class of Objects2Real function.
 *
 *
 */
/* ------------------------------------------------------------------------- */
class cl_Objects2Real
{
public:
	/* ------------------------------------------------------------------------- */
	/**	\fn public static void Objects2Real()
	 *
	 *  \brief
	 *
	 *  \param[in]
	 *  \param[in]
	 *
	 *  \param[out] bool
	 *
	 */
	/* ------------------------------------------------------------------------- */
	static cl_DynamicObjects Objects2Real(DistanceImage str_DistImage,
							 DetectedObjects str_DetectedObjects,
							 Matrix tf_Dref,
							 ros::Time ActTime)
	{
		DynObjConst str_DynObjConst = DynObjConst();
		pcl::PointCloud<pcl::PointXYZRGB> CollisionPoints= str_DistImage.CollisionPoints;
		int s_width = str_DistImage.s_width;
		int n_px = 0;
		int pos_i;
		int pos_j;
		int i_aux = 0;
		float mean_i = 0;
		float mean_j = 0;
		float difference_tol = str_DynObjConst.f_laser_reference_dif_tol;
		float xmin = 100000;
		float xmax = 0;
		float ymin = 100000;
		float ymax = 0;
		float zmax = 0;
		float zmin = 100000;
		float XCollision, YCollision, ZCollision;
		float x_tol = str_DynObjConst.f_x_size_tol;
		float y_tol = str_DynObjConst.f_y_size_tol;
		float z_tol = str_DynObjConst.f_z_size_tol;
		float f_size;
		int i_obj_added = 1;
		Matrix Objects  = str_DetectedObjects.m_Objects;
		/*Structure to store data*/
		cl_DynamicObjects RobotObjectList = cl_DynamicObjects();

		set<int>::iterator it;

		ROS_INFO("Objects 2 Real con %d labels",str_DetectedObjects.s_labels.size());
		for (it = str_DetectedObjects.s_labels.begin(); it != str_DetectedObjects.s_labels.end(); it++)
		{
			int index[100000][2];
			n_px = 0;
			i_aux = 0;
			for (int i = 1; i <= str_DistImage.s_height; i++)
			{
				for (int j = 1; j <= str_DistImage.s_width; j++)
				{
					if (Objects(i, j) == *it)
					{
						index[i_aux][0] = i;
						index[i_aux][1] = j;
						n_px++;
						i_aux++;
					}
				}
			}
			//ROS_INFO("busqueda pixeles objeto %d, con %d pixeles",*it,n_px);
			//Check all the points in the object to see if
			// they are part of the object itself or the
			// background.
			mean_i = 0;
			mean_j = 0;
			xmin = 100000;xmax = -100000;
			ymin = 100000;ymax = -100000;
			zmin = 100000;zmax = -100000;
			for (int i = 0; i < n_px; i++)
			{
				pos_i = index[i][0];
				pos_j = index[i][1];
				if (fabs(str_DistImage.tf_Distance(pos_i, pos_j) - tf_Dref(pos_i, pos_j)) > difference_tol)
				{

					mean_i = mean_i + pos_i;
					mean_j = mean_j + pos_j;
					/*The point belongs to the object*/
					if (!std::isnan(CollisionPoints.points[(pos_i-1)*s_width + pos_j].x))
					{
						XCollision = CollisionPoints.points[(pos_i-1)*s_width + pos_j].x;
						YCollision = CollisionPoints.points[(pos_i-1)*s_width + pos_j].y;
						ZCollision = CollisionPoints.points[(pos_i-1)*s_width + pos_j].z;
						ROS_INFO("Collision %f, %f, %f",XCollision,YCollision,ZCollision);
						/*They are stored in*/
						xmin = std::min(XCollision, xmin);
						xmax = std::max(XCollision, xmax);
						ymin = std::min(YCollision, ymin);
						ymax = std::max(YCollision, ymax);
						zmin = std::min(ZCollision, zmin);
						zmax = std::max(ZCollision, zmax);
					}
					else
					{
					//  ROS_INFO("Pixel no válido");
					}
				}
				else
				{
				//  ROS_INFO("Diferencia con TDref no válida");
				}
			}
			if ((xmax - xmin) > x_tol || (ymax - ymin) > y_tol || (zmax - zmin) > z_tol)
			{

				f_size = std::max(std::max(fabs(xmax - xmin), fabs(ymax - ymin)), fabs(zmax - zmin));
				cl_DynamicObjectList auxObject;
				auxObject.PhysCharac.f_PhysSize = f_size;
	//			str_RobotObjectList.DynamicObjects.List[i_obj_added] = new Interfaces.Complex.cl_DynamicObjectList();
	//			str_RobotObjectList.DynamicObjects.List[i_obj_added].PhysCharac.f_PhysSize = f_size;
				//Calculate the position of the center of the robot
				//Position
				pos_i = (int)floor((double)mean_i / n_px);
				pos_j = (int)floor((double)mean_j / n_px);
				//ROS_INFO("Central Cell %d -%d",pos_i,pos_j);
				if (!std::isnan(CollisionPoints.points[(pos_i-1)*s_width + pos_j].x))
				{
					auxObject.Pos3D.f_x = CollisionPoints.points[(pos_i-1)*s_width + pos_j].x;
					auxObject.Pos3D.f_y = CollisionPoints.points[(pos_i-1)*s_width + pos_j].y;
					auxObject.Pos3D.f_z = CollisionPoints.points[(pos_i-2)*s_width + pos_j].z;
					auxObject.PhysCharac.RColor = CollisionPoints.points[(pos_i-2)*s_width + pos_j].r;
					auxObject.PhysCharac.GColor = CollisionPoints.points[(pos_i-2)*s_width + pos_j].g;
					auxObject.PhysCharac.BColor = CollisionPoints.points[(pos_i-2)*s_width + pos_j].b;
					//str_RobotObjectList.DynamicObjects.List[i_obj_added].Position.d_x = CollisionMatrix[(pos_i-2)*s_width + pos_j-1].f_x;
					//str_RobotObjectList.DynamicObjects.List[i_obj_added].Position.d_y = CollisionMatrix[(pos_i-2)*s_width + pos_j-1].f_z;
				}
				else
				{
					auxObject.Pos3D.f_x = 0;
					auxObject.Pos3D.f_y = 0;
					auxObject.Pos3D.f_z = 0;
				//	str_RobotObjectList.DynamicObjects.List[i_obj_added].Position.d_x = 0;
				//	str_RobotObjectList.DynamicObjects.List[i_obj_added].Position.d_y = 0;
				}
				auxObject.r_DetectionTime = ActTime;
				RobotObjectList.List.push_back(auxObject);
			//	str_RobotObjectList.DynamicObjects.List[i_obj_added].f_DetectionTime = ActTime;
				i_obj_added++;
			}
		}

//		DynObjaux = new cl_DynamicObjects((ushort)i_obj_added);
//		i_aux = 0;
//		int n1 = Convert.ToInt32(Convert.ToString(RobotID[7]));
//		int n2 = Convert.ToInt32(Convert.ToString(RobotID[8]));
//		ushort Obj_ID = (ushort)(n1 * 10000 + n2 * 1000);
//		for (int i = 0; i < str_DetectedObjects.i_NumberOfObjects; i++)
//		{
//			if (str_RobotObjectList.DynamicObjects.List[i] != null)
//			{
//				DynObjaux.List[i_aux] = str_RobotObjectList.DynamicObjects.List[i];
//				DynObjaux.List[i_aux].us_Identification = (ushort)(Obj_ID+1);
//				Obj_ID++;
//				i_aux++;
//
//			}
//		}
//		str_RobotObjectList.DynamicObjects = DynObjaux;
//
//		return str_RobotObjectList;
		return RobotObjectList;
	}


};
}



//
//cl_3DLrfMeasure Lrf3DMeasure,
//                                                        DetectedObjects str_DetectedObjects,
//                                                        float[,] tf_Dref,
//                                                        cl_RobotEstimate RobotEstimate,
//                                                        cl_Pos3D[,] CollisionMatrix,
//                                                        float ActTime, char[] RobotID
//                                                      )
//        {
//
//            /*Variables*/
//            RobotObjectList str_RobotObjectList = new RobotObjectList(30);
//            int[,] index = new int[1000, 2];
//            int n_px = 0;
//            int pos_i;
//            int pos_j;
//            int i_aux = 0;
//            float mean_i = 0;
//            float mean_j = 0;
//            float difference_tol = DynObj_Const.f_laser_reference_dif_tol;
//            float xmin = 100000;
//            float xmax = 0;
//            float ymin = 100000;
//            float ymax = 0;
//            float zmax = 0;
//            float zmin = 100000;
//            float XCollision, YCollision, ZCollision;
//            float x_tol = DynObj_Const.f_x_size_tol;
//            float y_tol = DynObj_Const.f_y_size_tol;
//            float z_tol = DynObj_Const.f_z_size_tol;
//            float f_size;
//            int i_obj_added = 0;
//            int[,] Objects = str_DetectedObjects.ti_Objects;
//            /*Structure to store data*/
//            str_RobotObjectList.DynamicObjects = new cl_DynamicObjects((ushort)str_DetectedObjects.i_NumberOfObjects);
//            cl_DynamicObjects DynObjaux;
//            for (int i_obj = 0; i_obj < str_DetectedObjects.i_NumberOfObjects; i_obj++)
//            {
//                index = new int[100000, 2];
//                n_px = 0;
//                i_aux = 0;
//                for (int i = 0; i < Lrf3DMeasure.s_i; i++)
//                {
//                    for (int j = 0; j < Lrf3DMeasure.s_n; j++)
//                    {
//                        if (Objects[i, j] == (int)str_DetectedObjects.s_labels[i_obj])
//                        {
//                            index[i_aux, 0] = i;
//                            index[i_aux, 1] = j;
//                            n_px++;
//                            i_aux++;
//                        }
//                    }
//                }
//                        /* Check all the points in the object to see if
//                         * they are part of the object itself or the
//                         * background.
//                         */
//                mean_i = 0;
//                mean_j = 0;
//                xmin = 100000;
//                xmax = -100000;
//                ymin = 100000;
//                ymax = -100000;
//                zmin = 100000;
//                zmax = -100000;
//                for (int i = 0; i < n_px; i++)
//                {
//                    pos_i = index[i, 0];
//                    pos_j = index[i, 1];
//
//
//                    if (Math.Abs(Lrf3DMeasure.tf_3DLrfDistance[pos_i, pos_j] - tf_Dref[pos_i, pos_j]) > difference_tol)
//                    {
//                        mean_i = mean_i + pos_i;
//                        mean_j = mean_j + pos_j;
//                        /*The point belongs to the object*/
//                        if (CollisionMatrix[pos_i, pos_j] != null)
//                        {
//                            XCollision = CollisionMatrix[pos_i, pos_j].f_x;
//                            YCollision = CollisionMatrix[pos_i, pos_j].f_y;
//                            ZCollision = CollisionMatrix[pos_i, pos_j].f_z;
//
//                            /*They are stored in*/
//                            xmin = Math.Min(XCollision, xmin);
//                            xmax = Math.Max(XCollision, xmax);
//                            ymin = Math.Min(YCollision, ymin);
//                            ymax = Math.Max(YCollision, ymax);
//                            zmin = Math.Min(ZCollision, zmin);
//                            zmax = Math.Max(ZCollision, zmax);
//                        }
//                    }
//                }
//
//                if ((xmax - xmin) > x_tol || (ymax - ymin) > y_tol || (zmax - zmin) > z_tol)
//                {
//
//                    f_size = Math.Max(Math.Max(Math.Abs(xmax - xmin), Math.Abs(ymax - ymin)), Math.Abs(zmax - zmin));
//                    str_RobotObjectList.DynamicObjects.List[i_obj_added] = new Interfaces.Complex.cl_DynamicObjectList();
//
//                    str_RobotObjectList.DynamicObjects.List[i_obj_added].PhysCharac.f_PhysSize = f_size;
//                    //Calculate the position of the center of the robot
//                    //Position
//                    pos_i = (int)Math.Round((double)mean_i / n_px);
//                    pos_j = (int)Math.Round((double)mean_j / n_px);
//                    if (CollisionMatrix[pos_i, pos_j] != null)
//                    {
//                        str_RobotObjectList.DynamicObjects.List[i_obj_added].Position.d_x = CollisionMatrix[pos_i, pos_j].f_x;
//                        str_RobotObjectList.DynamicObjects.List[i_obj_added].Position.d_y = CollisionMatrix[pos_i, pos_j].f_z;
//                    }
//                    else
//                    {
//                        str_RobotObjectList.DynamicObjects.List[i_obj_added].Position.d_x = 0;
//                        str_RobotObjectList.DynamicObjects.List[i_obj_added].Position.d_y = 0;
//                    }
//                    str_RobotObjectList.DynamicObjects.List[i_obj_added].f_DetectionTime = ActTime;
//                    i_obj_added++;
//                }
//            }
//            DynObjaux = new cl_DynamicObjects((ushort)i_obj_added);
//            i_aux = 0;
//            int n1 = Convert.ToInt32(Convert.ToString(RobotID[7]));
//            int n2 = Convert.ToInt32(Convert.ToString(RobotID[8]));
//            ushort Obj_ID = (ushort)(n1 * 10000 + n2 * 1000);
//            for (int i = 0; i < str_DetectedObjects.i_NumberOfObjects; i++)
//            {
//                if (str_RobotObjectList.DynamicObjects.List[i] != null)
//                {
//                    DynObjaux.List[i_aux] = str_RobotObjectList.DynamicObjects.List[i];
//                    DynObjaux.List[i_aux].us_Identification = (ushort)(Obj_ID+1);
//                    Obj_ID++;
//                    i_aux++;
//
//                }
//            }
//            str_RobotObjectList.DynamicObjects = DynObjaux;
//
//            return str_RobotObjectList;

